#include "struct_typedef.h"
#include "pid.h"
#include "bmi088.h"
#include "user_math.h"
#include "can_use.h"
#include "cmd.h"

#define PTZ_OK (0)        /* 运行正常 */
#define PTZ_ERR (-1)      /* 运行时发现了其他错误 */
#define PTZ_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define PTZ_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
#define PTZ_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */


/*云台方向初始化设置*/
typedef enum {
	PTZ_INIT_SET_ZERO,  //零点
	PTZ_INIT_DONT_SET_ZERO,  //不设置零点
}PTZ_InitSetZero_e;


/*云台控制器状态*/
typedef enum {
    PTZ_CTRL_STOP,      //停止
    PTZ_CTRL_RUN,       //运行
    PTZ_CALIBRATION,    //校准
    PTZ_CTRL_ERROR,     //错误
}PTZ_Controller_Status_e;


/* 该结构体用于存放云台的一些固定参数在config.c中更改后不再变化 */
typedef struct
{   
	pid_param_t	PTZ_3508_spd_pargam;		//云台速度pid
	pid_param_t	PTZ_3508_ang_pargam;		//云台角度pid
	PTZ_InitSetZero_e PTZ_zero_set;			//云台是否将初始化位置设置为编码器零点
}PTZ_Param_t;



/*云台角度*/
typedef struct 
{
	uint8_t PTZ_task_run;              //线程的运行
	const PTZ_Param_t *param;          //一些固定的参数
	PTZ_Controller_Status_e status;    //控制器状态
	float PTZ_angle;                   //云台角度
	float PTZ_omega;        	       //云台角速度

	int16_t final_3508out;             //最终经由PID计算出的发送给电机的实际输出值

	struct{
    fp32 rotor_rpm3508[1];  		   //电机转速
	fp32 rotor_angle3508[1];			//电机角度
	fp32 m3508_current[1];      		//电机电流
	}motorfeedback;                     //电机反馈结构体

	struct{
	pid_type_def PTZ_3508anglePid[1];     //云台角度pid    
	pid_type_def PTZ_3508SpdPid[1];     //云台速度pid               
	}pid;

}PTZ_t;



/**
 *  @brief  运行的主结构体，所有这个文件里的函数都在操作这个结构体
 * 包含了初始化参数，中间变量，输出变量
 */
typedef struct{
	
	uint8_t PTZ_task_run;                  //线程的运行

   const	PTZ_Param_t *param;        //一些固定的参数

	PTZ_Controller_Status_e status;    //控制器状态
    ChassisMove_Vec move_vec;         //鐢辨帶鍒朵换鍔″喅瀹�

    struct{
    fp32 rotor_rpm6020[4];
	  fp32 rotor_angle6020[4];
	
	  fp32 rotor_rpm5065[4];
	  fp32 rotor_angle5065[4];
	  fp32 torque_current[4];
	}motorfeedback;
    

	/*鏈熸湜鐨勫簳鐩樿緭鍑哄€�(姝ゅ�勪负鑸佃疆瑙ｇ畻鍑虹殑鍚勪釜鐢垫満鐨勬湡鏈涜緭鍑哄€�)值*/
	struct{
	fp32 AgvAngleOut[4];                         //
	fp32 AgvSpeedOut[4];                       //
	}hopemotorout;

	/*缁廝ID璁＄畻鍚庣殑瀹為檯鍙戦€佺粰鐢垫満鐨勫疄鏃惰緭鍑哄€�*/
    struct 
	{
		fp32 final_6020out[4];             //鏈€缁堢粡鐢盤ID璁＄畻鍑虹殑鍙戦€佺粰鐢垫満鐨勫疄闄呰緭鍑哄€�
	   fp32 final_5065out[4];             //
	}final_out;
	
	struct{
	pid_type_def chassis_6020anglePid[4];                       
	pid_type_def chassis_6020OmegaPid[4];                     
	pid_type_def chassis_AngleCorPID;                         
	pid_type_def chassis_OmegaCorPID;                       
	}pid;

}Chassis_t;

